#include <ros/ros.h>  
#include <std_msgs/Bool.h> 

#include "balloon_rl/safe_serial.hpp"
#include "balloon_rl/serial_protocol.hpp"
#include "balloon_rl/data_classifier.hpp"

SafeSerial::SafeSerialHandle ssport;
SerialProtocol::data_unpack msgUnpacker;
SerialProtocol::data_pack msgPacker;

int looprate;

balloon_rl::nx_from_mcu msg_MCU;

void node_param_init(ros::NodeHandle &n)
{
    int bandrate;
    std::string serial_port;
    n.getParam("serial_port", serial_port);
    n.getParam("bandrate", bandrate);
    n.getParam("looprate", looprate);

    ssport.setport(serial_port, bandrate);
    ssport.init();
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "nx_serial_receive_node");
    ros::NodeHandle n("~");
    ros::NodeHandle n_;

    node_param_init(n);
    ros::Rate loop_rate(looprate);

    ros::Publisher topicMcuData = n_.advertise<balloon_rl::nx_from_mcu>("MCU_DATA_receive", 20);

    ROS_INFO("serial_receive_node created!");

    while (ros::ok())
    {
        ros::spinOnce();

        if(ssport.check()) //safeSerial
        {
            std::string databuff;
            size_t datalen = ssport.available();
            // ROS_INFO("data_len = %d", datalen);

            if(datalen != 0)
            {
                databuff = ssport.read(datalen);

                for(int i = 0; i <= (int)datalen - 1; i++)
                {
                    // ROS_INFO("%c", databuff[i]);
                    if(msgUnpacker.push(databuff[i]) == SerialProtocol::DATA_READY)
                    {
                        // ROS_INFO("Receive Node CRC16 check pass");

                        uint8_t dataLen = msgUnpacker.getDataLen();
                        std::vector<uint8_t> msgData = msgUnpacker.getData();

                        DataClassifier::msgID_e ID = DataClassifier::msgID_confirm(msgUnpacker.getID(), dataLen);
                        // ROS_INFO("ID: %d", ID);

                        // 有效数据提取
                        switch(ID)
                        {
                            case DataClassifier::RL_OBS_MSG:
                                DataClassifier::McuMsg_Unpack(&msg_MCU, msgData);
                                // output
                                topicMcuData.publish(msg_MCU);
                                break;

                            default:
                                break;
                        }
                    }
                }
            }
        }

        loop_rate.sleep();
    }

    ssport.close();
    
    return 0;
}